using BepuPhysics.Collidables;
using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.CollisionDetection.SweepTasks
{
    public interface IPairDistanceTester<TShapeWideA, TShapeWideB>
    {
        void Test(in TShapeWideA a, in TShapeWideB b, in Vector3Wide offsetB, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector<int> inactiveLanes,
            out Vector<int> intersected, out Vector<float> distance, out Vector3Wide closestA, out Vector3Wide normal);
    }

    public class ConvexPairSweepTask<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPairDistanceTester> : SweepTask
            where TShapeA : unmanaged, IConvexShape
            where TShapeB : unmanaged, IConvexShape
            where TShapeWideA : unmanaged, IShapeWide<TShapeA>
            where TShapeWideB : unmanaged, IShapeWide<TShapeB>
            where TPairDistanceTester : struct, IPairDistanceTester<TShapeWideA, TShapeWideB>
    {
        public ConvexPairSweepTask()
        {
            ShapeTypeIndexA = default(TShapeA).TypeId;
            ShapeTypeIndexB = default(TShapeB).TypeId;
        }
        protected override unsafe bool PreorderedTypeSweep(
            void* shapeDataA, in RigidPose localPoseA, in Quaternion orientationA, in BodyVelocity velocityA,
            void* shapeDataB, in RigidPose localPoseB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT,
            float minimumProgression, float convergenceThreshold, int maximumIterationCount,
            out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal)
        {
            OffsetSweep sweepModifier = default;
            sweepModifier.LocalPoseA = localPoseA;
            sweepModifier.LocalPoseB = localPoseB;
            return Sweep(
                shapeDataA, orientationA, velocityA,
                shapeDataB, offsetB, orientationB, velocityB,
                maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, ref sweepModifier,
                out t0, out t1, out hitLocation, out hitNormal);
        }

        protected override unsafe bool PreorderedTypeSweep<TSweepFilter>(
            void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA,
            void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB, float maximumT,
            float minimumProgression, float convergenceThreshold, int maximumIterationCount,
            bool requiresFlip, ref TSweepFilter filter, Shapes shapes, SweepTaskRegistry sweepTasks, BufferPool pool, out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal)
        {
            UnoffsetSweep sweepModifier = default;
            return Sweep(
                shapeDataA, orientationA, velocityA,
                shapeDataB, offsetB, orientationB, velocityB,
                maximumT, minimumProgression, convergenceThreshold, maximumIterationCount, ref sweepModifier,
                out t0, out t1, out hitLocation, out hitNormal);
        }

        static bool GetSphereCastInterval(in Vector3 origin, in Vector3 direction, float radius, out float t0, out float t1)
        {
            // 规格化方向。Sqrt并没有"那么"糟糕,而且它既简化了事情,又有助于避免数值问题。
            var dLength = direction.Length();
            if (dLength == 0)
            {
                // 零长度方向=>未命中或无限长度相交。
                t0 = 0;
                t1 = float.MaxValue;
                return origin.LengthSquared() <= radius * radius;
            }
            var inverseDLength = 1f / dLength;
            var d = direction * inverseDLength;

            // 将原点上移到尽可能早的撞击时间。由于数学原因,这并不是必需的,但它确实有助于避免一些数值问题。
            var tOffset = -Vector3.Dot(origin, d) - radius;
            if (tOffset < 0)
                tOffset = 0;
            var o = origin + d * tOffset;
            var b = Vector3.Dot(o, d);
            var c = Vector3.Dot(o, o) - radius * radius;

            if (b > 0 && c > 0)
            {
                // 雷在外面指着别处,没有击中。
                t0 = t1 = 0;
                return false;
            }

            var discriminant = b * b - c;
            if (discriminant < 0)
            {
                // 雷打偏了,没有命中。
                t0 = t1 = 0;
                return false;
            }
            var intervalRadius = (float)Math.Sqrt(discriminant);
            t0 = (tOffset - intervalRadius - b) * inverseDLength;
            t1 = (tOffset + intervalRadius - b) * inverseDLength;
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void GetSampleTimes(float t0, float t1, ref Vector<float> samples)
        {
            ref var sampleBase = ref Unsafe.As<Vector<float>, float>(ref samples);
            var sampleSpacing = (t1 - t0) * (1f / (Vector<float>.Count - 1));
            for (int i = 0; i < Vector<float>.Count; ++i)
            {
                Unsafe.Add(ref sampleBase, i) = t0 + i * sampleSpacing;
            }
        }

        interface ISweepModifier
        {
            bool GetSphereCastInterval(
                in Vector3 offsetB, in Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB,
                in Quaternion orientationA, in Vector3 angularVelocityA, float angularSpeedA,
                in Quaternion orientationB, in Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation);
            void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Vector3Wide angularA, ref Vector3Wide angularB,
                ref Vector3Wide initialOffsetB, ref QuaternionWide initialOrientationA, ref QuaternionWide initialOrientationB,
                ref Vector<float> samples, ref Vector3Wide sampleOffsetB, ref QuaternionWide sampleOrientationA, ref QuaternionWide sampleOrientationB);
            void GetNonlinearVelocityContribution(ref Vector3Wide normal,
                out Vector<float> velocityContributionA, out Vector<float> maximumDisplacementA,
                out Vector<float> velocityContributionB, out Vector<float> maximumDisplacementB);
            void AdjustHitLocation(in Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation);
        }

        struct UnoffsetSweep : ISweepModifier
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void AdjustHitLocation(in Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation)
            {
                hitLocation = hitLocation + t0 * velocityA.Linear;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Vector3Wide angularA, ref Vector3Wide angularB,
                ref Vector3Wide initialOffsetB, ref QuaternionWide initialOrientationA, ref QuaternionWide initialOrientationB,
                ref Vector<float> samples, ref Vector3Wide sampleOffsetB, ref QuaternionWide sampleOrientationA, ref QuaternionWide sampleOrientationB)
            {
                GetSampleTimes(t0, t1, ref samples);
                // 将OffsetB集成到样本位置。
                Vector3Wide.Scale(linearB, samples, out var displacement);
                Vector3Wide.Add(initialOffsetB, displacement, out sampleOffsetB);

                // 将方向整合到样本位置。
                var halfSamples = samples * 0.5f;
                PoseIntegration.Integrate(initialOrientationA, angularA, halfSamples, out sampleOrientationA);
                PoseIntegration.Integrate(initialOrientationB, angularB, halfSamples, out sampleOrientationB);
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool GetSphereCastInterval(
                in Vector3 offsetB, in Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB,
                in Quaternion orientationA, in Vector3 angularVelocityA, float angularSpeedA,
                in Quaternion orientationB, in Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation)
            {
                var hit = ConvexPairSweepTask<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPairDistanceTester>.GetSphereCastInterval(offsetB, linearVelocityB, maximumRadiusA + maximumRadiusB, out t0, out t1);
                hitLocation = offsetB + linearVelocityB * t0;
                hitNormal = Vector3.Normalize(-hitLocation); // 法线被校准为从B指向A。
                hitLocation += hitNormal * maximumRadiusB;
                return hit;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void GetNonlinearVelocityContribution(ref Vector3Wide normal,
                out Vector<float> velocityContributionA, out Vector<float> maximumDisplacementA,
                out Vector<float> velocityContributionB, out Vector<float> maximumDisplacementB)
            {
                velocityContributionA = Vector<float>.Zero;
                maximumDisplacementA = Vector<float>.Zero;
                velocityContributionB = Vector<float>.Zero;
                maximumDisplacementB = Vector<float>.Zero;
            }
        }
        struct OffsetSweep : ISweepModifier
        {
            public RigidPose LocalPoseA;
            public RigidPose LocalPoseB;
            public float TangentSpeedA;
            public float TangentSpeedB;
            public float TwiceRadiusA;
            public float TwiceRadiusB;
            public Vector3 AngularVelocityDirectionA;
            public Vector3 AngularVelocityDirectionB;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void AdjustHitLocation(in Quaternion initialOrientationA, in BodyVelocity velocityA, float t0, ref Vector3 hitLocation)
            {
                PoseIntegration.Integrate(new RigidPose { Orientation = initialOrientationA }, velocityA, t0, out var integratedPose);
                QuaternionEx.Transform(LocalPoseA.Position, integratedPose.Orientation, out var childOffset);
                hitLocation = hitLocation + integratedPose.Position + childOffset;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void ConstructSamples(float t0, float t1, ref Vector3Wide linearB, ref Vector3Wide angularA, ref Vector3Wide angularB,
                ref Vector3Wide initialOffsetB, ref QuaternionWide initialOrientationA, ref QuaternionWide initialOrientationB,
                ref Vector<float> samples, ref Vector3Wide sampleOffsetB, ref QuaternionWide sampleOrientationA, ref QuaternionWide sampleOrientationB)
            {
                GetSampleTimes(t0, t1, ref samples);
                // 将OffsetB集成到样本位置。
                Vector3Wide.Scale(linearB, samples, out var displacement);
                Vector3Wide.Add(initialOffsetB, displacement, out sampleOffsetB);

                // 请注意,初始方向是所属实体的属性,而不是子实体的属性。
                // 儿童自身的取向是localOrientation*body Orientation的产物。
                var halfSamples = samples * 0.5f;
                RigidPoses.Broadcast(LocalPoseA, out var localPosesA);
                PoseIntegration.Integrate(initialOrientationA, angularA, halfSamples, out var integratedOrientationA);
                Compound.GetRotatedChildPose(localPosesA, integratedOrientationA, out var childPositionA, out sampleOrientationA);

                RigidPoses.Broadcast(LocalPoseB, out var localPosesB);
                PoseIntegration.Integrate(initialOrientationB, angularB, halfSamples, out var integratedOrientationB);
                Compound.GetRotatedChildPose(localPosesB, integratedOrientationB, out var childPositionB, out sampleOrientationB);

                Vector3Wide.Subtract(childPositionB, childPositionA, out var netOffsetB);
                Vector3Wide.Add(sampleOffsetB, netOffsetB, out sampleOffsetB);
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool GetSphereCastInterval(
                in Vector3 offsetB, in Vector3 linearVelocityB, float maximumT, float maximumRadiusA, float maximumRadiusB,
                in Quaternion orientationA, in Vector3 angularVelocityA, float angularSpeedA,
                in Quaternion orientationB, in Vector3 angularVelocityB, float angularSpeedB, out float t0, out float t1, out Vector3 hitNormal, out Vector3 hitLocation)
            {
                // 切线速度大小在扫描过程中不会更改。计算并缓存它,作为偏移量贡献的上限。
                QuaternionEx.TransformWithoutOverlap(LocalPoseA.Position, orientationA, out var rA);
                var tangentA = Vector3.Cross(rA, angularVelocityA);
                TangentSpeedA = tangentA.Length();
                QuaternionEx.TransformWithoutOverlap(LocalPoseB.Position, orientationB, out var rB);
                var tangentB = Vector3.Cross(rB, angularVelocityB);
                TangentSpeedB = tangentB.Length();
                TwiceRadiusA = 2 * LocalPoseA.Position.Length();
                TwiceRadiusB = 2 * LocalPoseB.Position.Length();
                AngularVelocityDirectionA = angularSpeedA > 1e-8f ? angularVelocityA / angularSpeedA : new Vector3();
                AngularVelocityDirectionB = angularSpeedB > 1e-8f ? angularVelocityB / angularSpeedB : new Vector3();
                // 角速度引起的最大平移为180度,因此角运动引起的最大平移为2*半径。
                // 如果扫掠覆盖的持续时间足够短,以至于没有达到最大值,我们将使用基于外推切线速度的(松散)估计。
                var nonlinearExpansion = Math.Min(maximumT * (TangentSpeedA + TangentSpeedB), TwiceRadiusA + TwiceRadiusB);
                var offsetBIncludingChildPoses = offsetB + rB - rA;
                var hit = ConvexPairSweepTask<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPairDistanceTester>.GetSphereCastInterval(offsetBIncludingChildPoses, linearVelocityB, maximumRadiusA + maximumRadiusB + nonlinearExpansion, out t0, out t1);
                hitLocation = offsetBIncludingChildPoses + linearVelocityB * t0;
                hitNormal = Vector3.Normalize(-hitLocation); // 法线被校准为从B指向A。
                hitLocation += hitNormal * (maximumRadiusB + nonlinearExpansion);
                return hit;
            }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void GetNonlinearVelocityContribution(ref Vector3Wide normal,
                out Vector<float> velocityContributionA, out Vector<float> maximumDisplacementA,
                out Vector<float> velocityContributionB, out Vector<float> maximumDisplacementB)
            {
                // 恒定角速度会在平面上创建圆周运动。那个平面的法线是角速度方向。
                // 所有偏移角诱导运动都被约束到此平面。
                // 因此,我们可以将切向速度贡献沿着法线约束得相当紧密。
                // 最大位移也是如此-
                // 如果角速度垂直于法线,则位移不超过2*半径。当N*W/||W||点变为1时,最大位移变为0。
                // (请注意,您可以通过注意2*Radius仅在R/||R||*NOnPlane=1时命中2*Radius来更紧密地约束它。只是(到目前为止)还没有那么远。)
                Vector3Wide.Broadcast(AngularVelocityDirectionA, out var directionA);
                Vector3Wide.Dot(normal, directionA, out var dotA);
                var scaleA = Vector.SquareRoot(Vector.Max(Vector<float>.Zero, Vector<float>.One - dotA * dotA));
                velocityContributionA = new Vector<float>(TangentSpeedA) * scaleA;
                maximumDisplacementA = new Vector<float>(TwiceRadiusA) * scaleA;
                Vector3Wide.Broadcast(AngularVelocityDirectionB, out var directionB);
                Vector3Wide.Dot(normal, directionB, out var dotB);
                var scaleB = Vector.SquareRoot(Vector.Max(Vector<float>.Zero, Vector<float>.One - dotB * dotB));
                velocityContributionB = new Vector<float>(TangentSpeedB) * scaleB;
                maximumDisplacementB = new Vector<float>(TwiceRadiusB) * scaleB;
            }
        }

        static unsafe bool Sweep<TSweepModifier>(
            void* shapeDataA, in Quaternion orientationA, in BodyVelocity velocityA,
            void* shapeDataB, in Vector3 offsetB, in Quaternion orientationB, in BodyVelocity velocityB,
            float maximumT, float minimumProgression, float convergenceThreshold, int maximumIterationCount,
            ref TSweepModifier sweepModifier,
            out float t0, out float t1, out Vector3 hitLocation, out Vector3 hitNormal)
            where TSweepModifier : struct, ISweepModifier
        {
            ref var shapeA = ref Unsafe.AsRef<TShapeA>(shapeDataA);
            ref var shapeB = ref Unsafe.AsRef<TShapeB>(shapeDataB);
            // TODO：最好去掉这个毫无意义的零初始值(如果编译器最终不能去掉它的话)。
            var wideA = default(TShapeWideA);
            var wideB = default(TShapeWideB);
            // TODO：检查以确保JIT省略了分支。
            if (wideA.InternalAllocationSize > 0)
            {
                var memory = stackalloc byte[wideA.InternalAllocationSize];
                wideA.Initialize(new RawBuffer(memory, wideA.InternalAllocationSize));
            }
            if (wideB.InternalAllocationSize > 0)
            {
                var memory = stackalloc byte[wideB.InternalAllocationSize];
                wideB.Initialize(new RawBuffer(memory, wideB.InternalAllocationSize));
            }
            wideA.Broadcast(shapeA);
            wideB.Broadcast(shapeB);
            var pairTester = default(TPairDistanceTester);
            // 将间隔初始化为1)输入边界[0,最大T]和2)两个形状的包围球的扫掠撞击间隔中较紧的一个。
            // 请注意,两个扫描球体的相交间隔等同于对具有组合半径的球体执行单个光线投射。
            // 与传统的球射线测试不同,我们同时保持近距离和远距离的间隔。
            // 将shapeA的位置(在零)视为具有组合半径的目标球体。这个方向就是净直线运动。
            // 请注意,由于偏移,角运动会产生非线性相对位移。
            // 在假设角速度趋于相对较低的情况下,我们通过角运动在区间内的最大贡献来扩大测试半径。
            // 请注意,最大贡献出现在180度。
            var linearVelocityB = velocityB.Linear - velocityA.Linear;
            shapeA.ComputeAngularExpansionData(out var maximumRadiusA, out var maximumAngularExpansionA);
            shapeB.ComputeAngularExpansionData(out var maximumRadiusB, out var maximumAngularExpansionB);
            var angularSpeedA = velocityA.Angular.Length();
            var angularSpeedB = velocityB.Angular.Length();
            if (!sweepModifier.GetSphereCastInterval(
                offsetB, linearVelocityB, maximumT, maximumRadiusA, maximumRadiusB,
                orientationA, velocityA.Angular, angularSpeedA,
                orientationB, velocityB.Angular, angularSpeedB, out t0, out t1, out hitNormal, out hitLocation) || t0 > maximumT || t1 < 0)
            {
                // 边界球体不相交,或者相交间隔在请求的搜索间隔之外。
                hitLocation = default;
                hitNormal = default;
                // Console.WriteLine("Early Out.");
                return false;
            }
            // 将间隔夹紧到预期的搜索空间。
            if (t0 < 0)
                t0 = 0;
            if (t1 > maximumT)
                t1 = maximumT;

            // 我们现在有一个相对紧凑的括号(至少不会比涉及的形状大太多)。
            // 在较高级别上,以下扫描使用两个部分：
            // 1)通过计算每个点的距离和法线,然后拉出区间,从而保守地将区间端点向根推进
            // 在正常/距离和速度允许的范围内。
            // 2)如果任何样本找到交叉点,立即将T1一直拉到交叉点。
            // 继续采样,直到t1<t0并且保证未命中,或者直到该过程收敛到所需ε内的交叉点。

            // 有一些微妙之处使得这比典型的纯保守方法更快：
            // 1)使用"推测性"样本。样本不是在已知的安全地点取样,而是在事前被强行送出,然后在事后进行验证。
            // (要理解此验证,请将每个样本的距离和法线视为定义了一个保证没有交集的"安全"间隔。
            // 保守的样本可能只能前进到这一步,但更远的推测性样本可能会发现它的安全区间向后延伸。
            // 以重叠保守样本的安全区间。由于在安全间隔中不能有交集,因此您可以合并这些安全间隔并进行更紧密的绑定
            // 潜在的交叉点。)
            // 2)通过提供的寻根ε强制样本进行非零进展。换句话说,区间收窄并不是严格保守的。
            // 这意味着可能会完全错过低于某个阈值的交叉点,但也会对查找交叉点所需的迭代次数设置一个较低的上限。
            // 对于CCD或类似的目的,掠过碰撞几乎无关紧要,所以能够选择更大的ε是很好的。
            // 请注意,这并不意味着撞击时间不精确-撞击精度由另一个提供的ε控制,一旦检测到任何交叉点,
            // 间隔应以极快的速度缩小到任意精度。
            // 3)采样采用宽SIMD方式。Vector<Float>.Count样本是分布式的,而不是一次只执行一个样本。
            // (这与线性收敛速度与SIMD宽度无关,因为SIMD采样必须并行进行,但这一点很重要。)

            var tangentSpeedA = new Vector<float>(maximumRadiusA * angularSpeedA);
            var tangentSpeedB = new Vector<float>(maximumRadiusB * angularSpeedB);
            var maxAngularExpansionA = new Vector<float>(maximumAngularExpansionA);
            var maxAngularExpansionB = new Vector<float>(maximumAngularExpansionB);

            Vector3Wide.Broadcast(offsetB, out var initialOffsetB);
            QuaternionWide.Broadcast(orientationA, out var initialOrientationA);
            QuaternionWide.Broadcast(orientationB, out var initialOrientationB);
            Vector3Wide.Broadcast(linearVelocityB, out var wideLinearVelocityB);
            Vector3Wide.Broadcast(velocityA.Angular, out var wideAngularVelocityA);
            Vector3Wide.Broadcast(velocityB.Angular, out var wideAngularVelocityB);

            // 初始样本均匀分布在[t0,t1]上。
            Vector<float> samples = default;
            Vector3Wide sampleOffsetB = default;
            QuaternionWide sampleOrientationA = default;
            QuaternionWide sampleOrientationB = default;
            Vector3Wide normals = default;
            Vector3Wide closestA = default;
            Vector<float> distances = default;
            Vector<int> intersections = default;
            var minimumProgressionWide = new Vector<float>(minimumProgression);

            float next0 = t0;
            float next1 = t1;
            sweepModifier.ConstructSamples(t0, t1,
                ref wideLinearVelocityB, ref wideAngularVelocityA, ref wideAngularVelocityB,
                ref initialOffsetB, ref initialOrientationA, ref initialOrientationB,
                ref samples, ref sampleOffsetB, ref sampleOrientationA, ref sampleOrientationB);

            bool intersectionEncountered = false;
            int iterationIndex = 0;
            while (true)
            {
                pairTester.Test(wideA, wideB, sampleOffsetB, sampleOrientationA, sampleOrientationB, Vector<int>.Zero,
                    out intersections, out distances, out closestA, out normals);

                Vector3Wide.Dot(normals, wideLinearVelocityB, out var linearVelocityAlongNormal);
                sweepModifier.GetNonlinearVelocityContribution(ref normals,
                    out var nonlinearVelocityContributionA, out var nonlinearMaximumDisplacementA,
                    out var nonlinearVelocityContributionB, out var nonlinearMaximumDisplacementB);
                // 请注意,对于任何给定的时间跨度,实体上任意点的最大位移是有界的。
                // Time ToNext=距离/
                // (线性贡献+
                // MIN(切线速度A,最大角度扩展A/Time ToNext)+
                // min(TangentSpeedB,maxumAngularExpansionB/timeToNext))
                // 右侧对timeToNext的依赖需要一些额外的工作来解决。
                // 我们为形状A,形状B,然后是A和B构造一个最坏的情况,然后选择下一次最具攻击性的。
                // 例如,如果两个形状的旋转速度都足够快,以达到最大角度膨胀：
                // timeToNext=距离/(linearContribution+(maxumAngularExpansionA+maxumAngularExpansionB)/timeToNext)
                // timeToNext*linearContribution+timeToNext*(maxumAngularExpansionA+maxumAngularExpansionB)/timeToNext)=距离
                // timeToNext*linearContribution+(maxumAngularExpansionA+maxumAngularExpansionB)=距离
                // timeToNext=(距离-(maxumAngularExpansionA+maxumAngularExpansionB))/linearContribution
                var aWorstCaseDistances = Vector.Max(Vector<float>.Zero, distances - maxAngularExpansionA - nonlinearMaximumDisplacementA);
                var angularDisplacementB = maxAngularExpansionB + nonlinearMaximumDisplacementB;
                var bWorstCaseDistances = Vector.Max(Vector<float>.Zero, distances - angularDisplacementB);
                var bothWorstCaseDistances = Vector.Max(Vector<float>.Zero, aWorstCaseDistances - angularDisplacementB);
                // 除以零速度应该会产生一个很大的(但有限的)安全区间。
                // 负(分离)速度也可以用同样的方法处理。
                var divisionGuard = new Vector<float>(1e-15f);
                var bothWorstCaseNextTime = bothWorstCaseDistances / Vector.Max(divisionGuard, linearVelocityAlongNormal);
                var angularContributionA = nonlinearVelocityContributionA + tangentSpeedA;
                var angularContributionB = nonlinearVelocityContributionB + tangentSpeedB;
                var aWorstCaseNextTime = aWorstCaseDistances / Vector.Max(divisionGuard, (linearVelocityAlongNormal + angularContributionB));
                var bWorstCaseNextTime = bWorstCaseDistances / Vector.Max(divisionGuard, (linearVelocityAlongNormal + angularContributionA));
                var bestCaseNextTime = distances / Vector.Max(divisionGuard, linearVelocityAlongNormal + angularContributionA + angularContributionB);
                var timeToNext = Vector.Max(Vector.Max(bothWorstCaseNextTime, aWorstCaseNextTime), Vector.Max(bWorstCaseNextTime, bestCaseNextTime));
                var aWorstCasePreviousTime = aWorstCaseDistances / Vector.Max(divisionGuard, angularContributionB - linearVelocityAlongNormal);
                var bWorstCasePreviousTime = bWorstCaseDistances / Vector.Max(divisionGuard, angularContributionA - linearVelocityAlongNormal);
                var bestCasePreviousTime = distances / Vector.Max(divisionGuard, angularContributionA + angularContributionB - linearVelocityAlongNormal);
                var timeToPrevious = Vector.Max(Vector.Max(-bothWorstCaseNextTime, aWorstCasePreviousTime), Vector.Max(bWorstCasePreviousTime, bestCasePreviousTime));
                var safeIntervalStart = samples - timeToPrevious;
                var safeIntervalEnd = samples + timeToNext;
                var forcedIntervalEnd = samples + Vector.Max(timeToNext, minimumProgressionWide);
                if (intersections[0] < 0)
                {
                    // 第一个样本是相交的,不能用区间的睡觉推到这个迭代上。
                    next1 = samples[0];
                    intersectionEncountered = true;
                }
                else
                {
                    int firstIntersectingIndex = Vector<float>.Count;
                    for (int i = 0; i < Vector<float>.Count; ++i)
                    {
                        if (intersections[i] < 0)
                        {
                            // 找到一个交叉点。将间隔端点向上拉。看更多的样本是没有意义的。
                            firstIntersectingIndex = i;
                            Debug.Assert(samples[i] >= t0);
                            next1 = samples[i];
                            intersectionEncountered = true;
                            break;
                        }
                    }
                    int lastSafeIndex = 0;
                    for (int i = 0; i < firstIntersectingIndex; ++i)
                    {
                        lastSafeIndex = i;
                        var nextIndex = i + 1;
                        if (nextIndex < firstIntersectingIndex)
                        {
                            // 请注意,我们使用强制间隔来测试安全遍历。
                            // 这允许寻根器在病理情况下跳过小的交叉点以获得显著的加速。
                            if (safeIntervalStart[i + 1] > forcedIntervalEnd[i])
                            {
                                // 不能拿到下一个样品。我们已经竭尽全力了。
                                break;
                            }
                        }
                    }
                    // 请注意,有时安全间隔可能会超过T1,即使由于数字问题发生了交叉口。
                    // 这对扫荡来说并不是灾难性的,但这确实意味着我们不能在这里验证这种情况。
                    Debug.Assert((safeIntervalEnd[lastSafeIndex] >= t0) || !intersectionEncountered);
                    next0 = safeIntervalEnd[lastSafeIndex];
                    // 将最佳法线复制到输出变量中。我们将在下一次迭代中覆盖所有宽法线,但我们需要保留最佳猜测。
                    hitNormal = new Vector3(normals.X[lastSafeIndex], normals.Y[lastSafeIndex], normals.Z[lastSafeIndex]);
                    hitLocation = new Vector3(closestA.X[lastSafeIndex], closestA.Y[lastSafeIndex], closestA.Z[lastSafeIndex]);

                    if (!intersectionEncountered)
                    {
                        // 如果尚未检测到交叉点,我们可以向前拉T1以缩小样本范围。
                        for (int i = Vector<float>.Count - 1; i >= 0; --i)
                        {
                            next1 = safeIntervalStart[i];
                            // 请注意,我们使用强制间隔来测试安全遍历。
                            // 这允许寻根器在病理情况下跳过小的交叉点以获得显著的加速。
                            if (i > 0 && forcedIntervalEnd[i - 1] < next1)
                            {
                                // 不能拿到下一个样品。我们已经把T1推到了它能走的最远的地方。
                                break;
                            }
                        }
                    }
                }

                // 采样区域被初始化为非保守界。
                // 真正的采样间隔计算稍后才完成;这里只是为了让我们可以设置t0和t1,并可能提前完成。
                var sample0 = t0 + minimumProgression;
                var sample1 = t1 - minimumProgression;
                var previousIntervalSpan = t1 - t0;
                // 在纯线性投射的情况下,一直前进到保守界限可能会导致碰撞。这不会使检测到的t值出错,
                // 但这将意味着我们不能得到一个更准确的法线和最近的点。通过仅在epsilon内前进,强制进行另一次迭代。
                t0 = t0 + (next0 - t0) * 0.9999f;
                t1 = next1;

                var intervalSpan = t1 - t0;
                // 几种不同的终止条件：
                // 1)跨度反转,表示在整个搜索区间内找到了安全路径,没有交叉点
                // 2)存在交叉口,间隔跨度足够小,可以通过请求的epsilon。请注意,这意味着未命中不能在"收敛"时退出。
                // 3)区间根本没有缩小。当数值精度耗尽时,就会发生这种情况。继续下去没有意义,这台机器不能代表更好的东西了。
                // 4)超出迭代。
                // Console.WriteLine($"迭代缩小：{previousIntervalSpan/intervalSpan}");
                if (intervalSpan < 0 ||
                    (intersectionEncountered && intervalSpan < convergenceThreshold) ||
                    intervalSpan >= previousIntervalSpan ||
                    ++iterationIndex >= maximumIterationCount)
                {
                    break;
                }

                // 现在我们需要清理激进的采样间隔。去掉所有的倒置。
                if (sample0 < t0)
                    sample0 = t0;
                else if (sample0 > t1)
                    sample0 = t1;
                if (sample1 < t0)
                    sample1 = t0;
                else if (sample1 > t1)
                    sample1 = t1;

                var minimumSpan = minimumProgression * (Vector<float>.Count - 1);
                var sampleSpan = sample1 - sample0;
                if (sampleSpan < minimumSpan)
                {
                    // 我们已经到了单个样本挤在最小级数以下的地步。
                    // 尝试通过将样本0推回到保守界限来腾出空间。
                    sample0 = sample0 - (minimumSpan - sampleSpan);
                    if (sample0 < t0)
                        sample0 = t0;
                    sampleSpan = sample1 - sample0;
                    // 现在检查是否需要将sample1移向t1。
                    // 请注意,我们从未将sample1一直推送到t1。T1通常在交叉点上,所以在那里取另一个样本是没有价值的。
                    // 取而代之的是,我们只往上走了一半。
                    if (sampleSpan < minimumSpan)
                        sample1 = sample1 + Math.Min(minimumSpan - sampleSpan, (t1 - sample1) * 0.5f);
                }

                // 样本界现在被约束为保守界的积极子集。
                sweepModifier.ConstructSamples(sample0, sample1,
                    ref wideLinearVelocityB, ref wideAngularVelocityA, ref wideAngularVelocityB,
                    ref initialOffsetB, ref initialOrientationA, ref initialOrientationB,
                    ref samples, ref sampleOffsetB, ref sampleOrientationA, ref sampleOrientationB);
            }
            // Console.WriteLine($"迭代计数：{iterationIndex}");
            // 如果有交叉点,我们需要更正样本位置的命中位置。
            sweepModifier.AdjustHitLocation(orientationA, velocityA, t0, ref hitLocation);
            return intersectionEncountered;
        }


    }
}
